/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#include <math.h>
#include "Mapper.hpp"


template <> TMapBlock<char>::TMapBlock(float x, float y) //float resolution)
  :x(x),
y(y),
userData(NULL)
//resolution(resolution)
{
  for (int i=0;i<BLOCK_DATA_SIZE;i++)
    data[i]=UNMAPPED_AREA_VALUE;
}

template <> TMapBlock<float>::TMapBlock(float x, float y) //float resolution)
  :x(x),
y(y),
userData(NULL)
//resolution(resolution)
{
  for (int i=0;i<BLOCK_DATA_SIZE;i++)
    data[i]=UNMAPPED_AREA_VALUE;
}

template <class type> TMapBlock<type>::TMapBlock(float x, float y) //float resolution)
  :x(x),
y(y),
userData(NULL)
//resolution(resolution)
{

}


/***********/


CMapper::CMapper()
:reso(-1),
type(-1)
{
  
}

CMapper::~CMapper()
{
  
}

bool CMapper::Update(const MaCI::Map::CMapData &mapData, gmbVector &aGroundBlocks,
                          occuVector &aOccuBlocks)
{
  dPrint(1,"1");
  const MaCI::Map::TMapUpdateHeader *header;
  const char * data;
    
  
  
  if (reso<0)
  {
    reso=mapData.GetMapHeader()->gridDataResolution;
  }
  dPrint(1,"2");
  header=mapData.GetMapUpdateHeader();
  //dPrint(1,"received map data! reso=%f clear=%d",header->resolution,header->clear);
  
  if (header && header->clear)
  {
    clearBlocks();
    mapLines.clear();
    areas.clear();
  }
  
  const MaCI::Map::TGridBlockHeader *block;
  dPrint(1,"3");
  for (int i=0;i<mapData.GetGridBlockCount();i++)
  {
    block=mapData.GetGridBlockHeader(i);
    data = mapData.GetGridBlockData(i);
    
    if (block->dataType==MaCI::Map::KGridTypeHeightMap)
    {
      updateGroundModel(block->x-block->dataOriginX*reso,
                        block->y-block->dataOriginY*reso,
                        block->samples_x, block->samples_y, (const float*)data, aGroundBlocks);
                      //block->width,block->height,header->resolution,data);
    } else if (block->dataType==MaCI::Map::KGridTypeOccupancy) {
      updateOccuModel(block->x-block->dataOriginX*reso,
                        block->y-block->dataOriginY*reso,
                        block->samples_x, block->samples_y, data, aOccuBlocks);
    }
  }
  dPrint(1,"4");
  for (int i=0;i<mapData.GetMapLineCount();i++)
  {
    mapLines.push_back(*(mapData.GetMapLine(i)));
  };
  
  for (int i=0;i<mapData.GetAreaCount();i++)
  {
    areas.push_back(*(mapData.GetArea(i)));
  };
  
  //dPrint(1,"update end");
  dPrint(1,"5");
  return true;
}

gmbMap* CMapper::GetGroundModel() const
{
  return (gmbMap*)&groundBlocks;
}

occuMap* CMapper::GetOccuModel() const
{
  return (occuMap*)&occuBlocks;
}

float CMapper::GetResolution() const
{
  return reso;
}

std::vector<MaCI::Map::TMapLine>* CMapper::GetMapLines()
{
  return &mapLines;
}

std::vector<MaCI::Map::TArea>* CMapper::GetAreas()
{
  return &areas;
}

void CMapper::updateGroundModel(const float aPosX, const float aPosY, 
                             const int data_samples_x, const int data_samples_y,
                                 //const float aWidth, const float aDepth,
                             const float* aHmData, gmbVector &updatedBlocks)
{
  /*
  * ground model is divided into GROUND_MODEL_BLOCK_SIZE wide blocks. 
  * when new data is given, only affected blocks are changed  
  * 
  */
  
  //ground block limits that new data is located within
  int start_block_x =(int) floor(aPosX/reso/GROUND_MODEL_BLOCK_SIZE);
  int start_block_y =(int) floor(aPosY/reso/GROUND_MODEL_BLOCK_SIZE);
  int end_block_x =(int) floor((aPosX+reso*data_samples_x)/reso/GROUND_MODEL_BLOCK_SIZE);
  int end_block_y =(int) floor((aPosY+reso*data_samples_y)/reso/GROUND_MODEL_BLOCK_SIZE);

  //given data block's offset from first block's origin
  // and samples that make the end parts of row and column
  int start_offset_samples_x, end_samples_x;
  int start_offset_samples_y, end_samples_y;
  
  start_offset_samples_x=(aPosX/reso)-GROUND_MODEL_BLOCK_SIZE*start_block_x;
  start_offset_samples_y=(aPosY/reso)-GROUND_MODEL_BLOCK_SIZE*start_block_y;
  
  end_samples_x=GROUND_MODEL_BLOCK_SIZE*(end_block_x-start_block_x+1)-start_offset_samples_x-data_samples_x;
  end_samples_y=GROUND_MODEL_BLOCK_SIZE*(end_block_y-start_block_y+1)-start_offset_samples_y-data_samples_y;
  
  int samples_end_x=GROUND_MODEL_BLOCK_SIZE;
  int samples_end_y=GROUND_MODEL_BLOCK_SIZE;
  int samples_start_x=start_offset_samples_x;
  int samples_start_y=start_offset_samples_y;
  
  //go through affected blocks
  for (int block_x=start_block_x; block_x <= end_block_x; block_x++)
  {
    //if last column
    if (block_x==end_block_x)
      samples_end_x=GROUND_MODEL_BLOCK_SIZE-end_samples_x;
    
    samples_start_y=start_offset_samples_y;
    
    samples_end_y=GROUND_MODEL_BLOCK_SIZE;
    
    for (int block_y=start_block_y; block_y <= end_block_y; block_y++)
    {
      //if last row
      if (block_y==end_block_y)
        samples_end_y=GROUND_MODEL_BLOCK_SIZE-end_samples_y;
      
      TGroundModelBlock *block;
      //check if terrain exists on this point already
      if (!(block=getGroundBlock(block_x,block_y)))
      {
        block=new TGroundModelBlock(GROUND_MODEL_BLOCK_SIZE*block_x*reso,
                                     GROUND_MODEL_BLOCK_SIZE*block_y*reso);//,reso);
        setGroundBlock(block_x,block_y,block);
      }
      
      updatedBlocks.push_back(block);
      
      //set hm-values in every block
      for (int samples_x=samples_start_x; samples_x<samples_end_x; samples_x++)
      {
        for (int samples_y=samples_start_y; samples_y<samples_end_y; samples_y++)
        {
          //sample position in given data
          int data_x=(block_x-start_block_x)*GROUND_MODEL_BLOCK_SIZE
              +samples_x-start_offset_samples_x;
          int data_y=(block_y-start_block_y)*GROUND_MODEL_BLOCK_SIZE
              +samples_y-start_offset_samples_y;
          
          block->data[samples_y*GROUND_MODEL_BLOCK_SIZE+samples_x]=
                            aHmData[data_y*data_samples_x+data_x];
        }
      }
      
      samples_start_y=0;
    }
    samples_start_x=0;
  }
}

void CMapper::updateOccuModel(const float aPosX, const float aPosY, 
                                const int data_samples_x, const int data_samples_y,
                                 //const float aWidth, const float aDepth,
                                const char* aOccuData, occuVector& updatedBlocks)
{
  /*
  * ground model is divided into GROUND_MODEL_BLOCK_SIZE wide blocks. 
  * when new data is given, only affected blocks are changed  
  * 
  */
  
  //ground block limits that new data is located within
  int start_block_x =(int) floor(aPosX/reso/GROUND_MODEL_BLOCK_SIZE);
  int start_block_y =(int) floor(aPosY/reso/GROUND_MODEL_BLOCK_SIZE);
  int end_block_x =(int) floor((aPosX+reso*data_samples_x)/reso/GROUND_MODEL_BLOCK_SIZE);
  int end_block_y =(int) floor((aPosY+reso*data_samples_y)/reso/GROUND_MODEL_BLOCK_SIZE);

  //given data block's offset from first block's origin
  // and samples that make the end parts of row and column
  int start_offset_samples_x, end_samples_x;
  int start_offset_samples_y, end_samples_y;
  
  start_offset_samples_x=(aPosX/reso)-GROUND_MODEL_BLOCK_SIZE*start_block_x;
  start_offset_samples_y=(aPosY/reso)-GROUND_MODEL_BLOCK_SIZE*start_block_y;
  
  end_samples_x=GROUND_MODEL_BLOCK_SIZE*(end_block_x-start_block_x+1)-start_offset_samples_x-data_samples_x;
  end_samples_y=GROUND_MODEL_BLOCK_SIZE*(end_block_y-start_block_y+1)-start_offset_samples_y-data_samples_y;
  
  int samples_end_x=GROUND_MODEL_BLOCK_SIZE;
  int samples_end_y=GROUND_MODEL_BLOCK_SIZE;
  int samples_start_x=start_offset_samples_x;
  int samples_start_y=start_offset_samples_y;
  
  //go through affected blocks
  for (int block_x=start_block_x; block_x <= end_block_x; block_x++)
  {
    //if last column
    if (block_x==end_block_x)
      samples_end_x=GROUND_MODEL_BLOCK_SIZE-end_samples_x;
    
    samples_start_y=start_offset_samples_y;
    
    samples_end_y=GROUND_MODEL_BLOCK_SIZE;
    
    for (int block_y=start_block_y; block_y <= end_block_y; block_y++)
    {
      //if last row
      if (block_y==end_block_y)
        samples_end_y=GROUND_MODEL_BLOCK_SIZE-end_samples_y;
      
      TOccuBlock *block;
      //check if terrain exists on this point already
      if (!(block=getOccuBlock(block_x,block_y)))
      {
        block=new TOccuBlock(GROUND_MODEL_BLOCK_SIZE*block_x*reso,
                                    GROUND_MODEL_BLOCK_SIZE*block_y*reso);//,reso);
        setOccuBlock(block_x,block_y,block);
      }
      
      updatedBlocks.push_back(block);
      
      //set hm-values in every block
      for (int samples_x=samples_start_x; samples_x<samples_end_x; samples_x++)
      {
        for (int samples_y=samples_start_y; samples_y<samples_end_y; samples_y++)
        {
          //sample position in given data
          int data_x=(block_x-start_block_x)*GROUND_MODEL_BLOCK_SIZE
              +samples_x-start_offset_samples_x;
          int data_y=(block_y-start_block_y)*GROUND_MODEL_BLOCK_SIZE
              +samples_y-start_offset_samples_y;
          
          block->data[samples_y*GROUND_MODEL_BLOCK_SIZE+samples_x]=
                aOccuData[data_y*data_samples_x+data_x];
          
        }
      }
      
      samples_start_y=0;
    }
    samples_start_x=0;
  }
}


TGroundModelBlock* CMapper::getGroundBlock(int x, int y)
{
  _gmbMapIter i;
  if ((i=groundBlocks.find(TMBKey(x,y)))==groundBlocks.end())
  {
    return NULL;
  } else {
    return i->second;
  }
}

void CMapper::setGroundBlock(int x, int y, TGroundModelBlock* block)
{
  groundBlocks.insert(gmbPair(TMBKey(x,y), block));
}

TOccuBlock* CMapper::getOccuBlock(int x, int y)
{
  _occuMapIter i;
  if ((i=occuBlocks.find(TMBKey(x,y)))==occuBlocks.end())
  {
    return NULL;
  } else {
    return i->second;
  }
}

void CMapper::setOccuBlock(int x, int y, TOccuBlock* block)
{
  occuBlocks.insert(occuPair(TMBKey(x,y), block));
}

void CMapper::clearBlocks()
{
   for (_gmbMapIter i=groundBlocks.begin();i!=groundBlocks.end();i++)
    delete i->second;
  groundBlocks.clear();
  
   for (_occuMapIter i=occuBlocks.begin();i!=occuBlocks.end();i++)
    delete i->second;
  occuBlocks.clear();
  
}

TMBKey CMapper::getBlockCoords(float x, float y)
{
  TMBKey key(0,0);
  key.x =(int) floor(x/reso/GROUND_MODEL_BLOCK_SIZE);
  key.y =(int) floor(y/reso/GROUND_MODEL_BLOCK_SIZE);
  
  return key;
}

float CMapper::GetHeightMapValue(float x, float y)
{
  TMBKey key=getBlockCoords(x,y);
  _gmbMapIter i=groundBlocks.find( key );
  
  if (i==groundBlocks.end())
    return UNMAPPED_AREA_VALUE;
  
  TGroundModelBlock *b=i->second;
  int dx=(x/reso)-GROUND_MODEL_BLOCK_SIZE*key.x;
  int dy=(y/reso)-GROUND_MODEL_BLOCK_SIZE*key.y;
  return b->data[dy*GROUND_MODEL_BLOCK_SIZE+dx];
}

unsigned char CMapper::GetOccuValue(float x, float y)
{
  
  TMBKey key=getBlockCoords(x,y);
  _occuMapIter i=occuBlocks.find( key );
  
  if (i==occuBlocks.end())
    return UNMAPPED_AREA_VALUE;
  
  TOccuBlock *b=i->second;
  int dx=(x/reso)-GROUND_MODEL_BLOCK_SIZE*key.x;
  int dy=(y/reso)-GROUND_MODEL_BLOCK_SIZE*key.y;
  return b->data[dy*GROUND_MODEL_BLOCK_SIZE+dx];
}

